#ifndef KALMAN_2STATE_H_
#define KALMAN_2STATE_H_

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/random.hpp>

class kalman_2state
{

	boost::numeric::ublas::matrix<double> A;
	boost::numeric::ublas::matrix<double> H;
	boost::numeric::ublas::matrix<double> S;
	boost::numeric::ublas::matrix<double> K;
	boost::numeric::ublas::matrix<double> Z;
	boost::numeric::ublas::matrix<double> R;
	boost::numeric::ublas::matrix<double> Q;
	boost::numeric::ublas::matrix<double> Inn;


public:
	kalman_2state();
	kalman_2state(boost::numeric::ublas::matrix<double> _R,
				  boost::numeric::ublas::matrix<double> _Q,
				  double dt = 0.1);


	void update(boost::numeric::ublas::matrix<double> &_state,
				boost::numeric::ublas::matrix<double> &_state_estimate,
				boost::numeric::ublas::matrix<double> &_covariance,
				boost::numeric::ublas::matrix<double> _Z,
				boost::numeric::ublas::matrix<double> _ProcessNoise);

};

#endif